#include "serial.hpp"
#include "crc16.hpp"
#include "robot.hpp"
#include "fd.hpp"
#include "hogdetector.hpp"
#include "kcftracker.hpp"
#include "mytim.hpp"
#include "mythread.hpp"
#include "myevent.hpp"
#include "mylock.hpp"
#include <opencv2/opencv.hpp>
#include <math.h>
#include <unistd.h>

using namespace std;
using namespace cv;

typedef struct PID
{
	float P,I,D,KP,KI,KD,opt;
}PID;

PID RotatePID = {0,0,0,0.1,0.1,0,0};
PID TiltPID = {0,0,0,0.1,0.1,0,0};

#define OBS_SN_DIS  100
#define CUT_UP_DIS  45
#define CMD_CH_MIN 0
#define CMD_CH_OFF 127
#define CMD_CH_MAX 254
#define CMD_CH_NEG 50
#define CMD_CH_POS 204

#define VISION_FRAME_WIDTH 640
#define VISION_FRAME_HEIGHT 480
#define VISION_TARGET_STD_SIZE Size(200,100)
#define VISION_FIRE_ON_ACCURACY 180
#define VISION_RETARGET_ON_SPEED 3
#define FRAME_DELAY_FROM_LOST_T0_SCOUT 0
#define VISION_LOST_FRAME_COUNTER 4

#define PREPARE_PERIOD 1000
#define SCOUT_PERIOD 40000
#define SCOUT_INTERVAL 5000
#define SCOUT_DUTY_CYCLE 500

#define scale(num,min1,max1,min2,max2) ((num-min1)*(max2-min2)/(max1-min1)+min2)
#define distance(x,y) (sqrt((x)*(x)+(y)*(y)))
#define limit(val,min,max) (val=(val<min?min:(val>max?max:val)))
#define centerX(rect) (rect.x + rect.width/2.0f)
#define centerY(rect) (rect.y + rect.height/2.0f)

/********************************* ROBOT **************************************/
Robot robot("/dev/ttyS4");

typedef struct AppPara
{
	uint8_t fb,lr,rotate,pan,tilt,fire;
}AppPara;
AppPara para = {127, 127, 127, 127, 127, 0};

void SpeedTx(float fb, float lr, float rotate, float pan, float tilt)
{
	limit(fb, -127, 127);
	limit(lr, -127, 127);
	limit(rotate, -127, 127);
	limit(pan, -127, 127);
	limit(tilt, -127, 127);
	para.fb = 127 + fb;
	para.lr = 127 + lr;
	para.rotate = 127 + rotate;
	para.pan = 127 + pan;
	para.tilt = 127 + tilt;
}

void Cmd()
{
	robot.cmd(para.fb, para.lr, para.rotate, para.pan, para.tilt, para.fire);
}

/****************************** ULTRAS ***********************************/
typedef struct Ultras
{
	uint8_t front,back,left,right;
}Ultras;

Ultras ultras = {0, 0, 0, 0};

bool ObstacleClear()
{
	return ultras.front>CUT_UP_DIS && ultras.back>CUT_UP_DIS && ultras.left>CUT_UP_DIS && ultras.right>CUT_UP_DIS;
}

typedef enum
{
	Stop = 0,
	Forward = 0x01,
	Backward = 0x02,
	Left = 0x04,
	Right = 0x08,
	Rotate = 0x10
}Heading;
Heading heading = Stop;

void HeadingSM()
{
	switch(heading)
	{
		case Stop:
		if(ultras.front > CUT_UP_DIS)
			heading = Forward;
		else if(ultras.right > CUT_UP_DIS)
			heading = Right;
		else if(ultras.back > CUT_UP_DIS)
			heading = Backward;
		else if(ultras.left > CUT_UP_DIS)
			heading = Left;
		else
			heading = Stop;
		break;
		
		case Forward:
		if(ultras.front > CUT_UP_DIS)
			heading = Forward;
		else if(ultras.right > CUT_UP_DIS)
			heading = Right;
		else if(ultras.left > CUT_UP_DIS)
			heading = Left;
		else if(ultras.back > CUT_UP_DIS)
			heading = Backward;
		else
			heading = Stop;
		break;
		
		case Backward:
		if(ultras.back > CUT_UP_DIS)
			heading = Backward;
		else if(ultras.left > CUT_UP_DIS)
			heading = Left;
		else if(ultras.right > CUT_UP_DIS)
			heading = Right;
		else if(ultras.front > CUT_UP_DIS)
			heading = Forward;
		else
			heading = Stop;
		break;
		
		case Left:
		if(ultras.left > CUT_UP_DIS)
			heading = Left;
		else if(ultras.front > CUT_UP_DIS)
			heading = Forward;
		else if(ultras.back > CUT_UP_DIS)
			heading = Backward;
		else if(ultras.right > CUT_UP_DIS)
			heading = Right;
		else
			heading = Stop;
		break;
		
		case Right:
		if(ultras.right > CUT_UP_DIS)
			heading = Right;
		else if(ultras.front > CUT_UP_DIS)
			heading = Forward;
		else if(ultras.back > CUT_UP_DIS)
			heading = Backward;
		else if(ultras.left > CUT_UP_DIS)
			heading = Left;
		else
			heading = Stop;
		break;
		
		default:
		heading = Forward;
		break;
	}
}

void ObstacleAvoidance()
{
	switch(heading)
	{
		case Stop:
		para.fb = para.lr = para.rotate = para.pan = CMD_CH_OFF;
		para.fire = 0;
		break;
		
		case Forward:
		para.fb = CMD_CH_POS;
		para.lr = para.rotate = para.pan = CMD_CH_OFF;
		para.fire = 0;
		break;
		
		case Backward:
		para.fb = CMD_CH_NEG;
		para.lr = para.rotate = para.pan =  CMD_CH_OFF;
		para.fire = 0;
		break;
		
		case Left:
		para.lr = CMD_CH_NEG;
		para.fb = para.rotate = para.pan = CMD_CH_OFF;
		para.fire = 0;
		break;
		
		case Right:
		para.lr = CMD_CH_POS;
		para.fb = para.rotate = para.pan = CMD_CH_OFF;
		para.fire = 0;
		break;
	}
}

void UltrasAction()
{
	HeadingSM();
	ObstacleAvoidance();
}

#define ULTRAS_BUF_LEN 256
#define ULTRAS_FRAME_LEN 7
#define ULTRAS_HEADER 0x00
void* ultrasThreadFunction(void* p)
{
	Serial serial("/dev/ttyACM0");
	if(!serial.connect())
	{
		cout << "Ultras Thread Fail to Open Serial" << endl;
		return NULL;
	}
	uint8_t buf[ULTRAS_BUF_LEN];
	uint32_t len = 0;
	uint32_t ptr = 0;
	double t = 0;
	while(1)
	{
		t = cvGetTickCount();
		len += serial.rx(buf+len, ULTRAS_BUF_LEN-len);
		if(len >= ULTRAS_FRAME_LEN)
		{
			for(ptr = 0;ptr <= len - ULTRAS_FRAME_LEN;ptr++)
			{
				if(buf[ptr] == ULTRAS_HEADER)
				{
					if(CRC16Verify(buf+ptr,ULTRAS_FRAME_LEN))
					{
						ultras.front = buf[ptr+1];
						ultras.back = buf[ptr+2];
						ultras.left = buf[ptr+3];
						ultras.right = buf[ptr+4];
						t = cvGetTickCount();
						//cout << "**************************** ULTRAS **********************************"<<endl;
						//cout << "FRONT: " << (int)ultras.front << ",BACK: " << (int)ultras.back << ",LEFT: " << (int)ultras.left << ",RIGHT: " << (int)ultras.right << endl;
						//cout << "TIME COST                                        " << (t/(cvGetTickFrequency()*1000)) << endl;
						//cout << "*********************************************************************"<<endl;
					}
				}
			}
			ptr--;
			uint32_t new_len = len - ptr;
			uint8_t tmp[new_len];
			memcpy(tmp, buf+ptr, new_len);
			memcpy(buf, tmp, new_len);
			len = new_len;
		}
	}
}

/***************************** VISION ******************************/
typedef struct Vision{
	int found;
	Mat image;
	Rect target;
	float x,y,z,f;
}Vision;

Vision vision;

int VisionFound()
{
	return vision.found;
}




#define INTE_CUT 20
#define INTE_MAX 50
float inte = 0;
float pid_rotate_out = 0;
void VisionPID()
{
	if(abs(vision.x) < INTE_CUT)
	{
		inte += vision.x;
	}
	else
	{
		inte = 0;
	}
	limit(inte, -INTE_MAX, INTE_MAX);
	pid_rotate_out = (0.13*vision.x + 0.0001*inte) + 127;
	limit(pid_rotate_out,80,174);
}

#define RAMP_LEN 50
int ramp_count = 0;

void VisionAction()
{
	if(ramp_count < RAMP_LEN)ramp_count++;
	float rate = ramp_count / (float)RAMP_LEN;
	
	//VisionPID();
	//para.rotate = pid_rotate_out;
	para.rotate = scale(rate*vision.x, -320, 320,80, 174);
	//cout << "inte                  " << inte << endl;
	//para.rotate = scale(rate*vision.x, -320, 320,80, 174) + scale(rate*inte, -INTE_MAX, INTE_MAX,100, 154) ;
	//para.rotate = 254 - scale(rate*vision.x, -320, 320, 100, 154);
	//para.tilt = 254 - scale(vision.y, -240, 240, 50, 204);
	para.tilt = 127 - 0.01*rate*vision.y;
	para.fb = 127;
	//para.fb = 127/(float)vision.z;
	//cout << vision.target.width << "    ********     " << vision.target.height << endl;
	para.fire = vision.f < VISION_FIRE_ON_ACCURACY ? 0x07 : 0x00;
	//cout << "f=" << vision.f << endl;
	para.lr = 127;
	para.pan = 127;
}

void* visionThreadFunction(void* p)
{
	VideoCapture cap(0);
	if(!cap.isOpened())
	{
		cout << "Vision Thread Fail to Capture" << endl;
		return NULL;
	}
	FD motion;
	HOGDetector detector;
	detector.load("model.xml");
	KCFTracker tracker;
	static Rect roi, last_roi, obj;
	static int trackingDeltaX, trackingDeltaY;
	static double trackingDeltaS = 0;
	static double mins,tmps;
	static uint32_t lost_counter = 0;
	static bool lost = true;
	Mat frame;
	cap >> frame;
	motion.init(frame);
	vector<Rect> objs;
	//double t = 0;
	while(1)
	{
		//t = cvGetTickCount();
		cap >> frame;
		vision.image = frame;
		int width = frame.cols;
		int height = frame.rows;
		int halfWidth = width / 2.0f;
		int halfHeight = height / 2.0f;
		if(lost)
		{
			//motion detection
			//cout << "motion" << endl;
			roi = motion.detect(frame);
			//cout << roi.width << "," << roi.height << endl;
			if (roi.width >80 && roi.height>40 && roi.width < 320 && roi.height < 240)
			{
				//appearence detection
				objs = detector.detect(frame(roi));
				if (objs.size() > 0) 
				{
					mins = distance(halfWidth, halfHeight);
					for (int i = 0; i < objs.size(); i++) 
					{
						tmps = distance(objs[i].x - halfWidth, objs[i].y - halfHeight);
						if (tmps < mins) 
						{
							mins = tmps;
							obj = objs[i];
						}
					}
					roi.x+=obj.x;roi.y+=obj.y;
					roi.width=obj.width;roi.height=obj.height;
					tracker.init(roi, frame); //important
					trackingDeltaS = VISION_RETARGET_ON_SPEED; //important
					rectangle(frame, roi, Scalar(255,0,0), 3);
					lost = false;
				}
			 }
		}
	    else 
		{
			last_roi = roi;
			roi = tracker.update(frame); 
			if(roi.width < 80 || roi.height < 40 || roi.x+roi.width > frame.cols || roi.y+roi.height > frame.rows)
			{
				lost = true;
			}
			else
			{
				trackingDeltaX = centerX(roi) - centerX(last_roi);
				trackingDeltaY = centerY(roi) - centerY(last_roi);
				trackingDeltaS = distance(trackingDeltaX, trackingDeltaY);
				if(trackingDeltaS < VISION_RETARGET_ON_SPEED)
				{
					//cout << "DS = " << trackingDeltaS << endl;
					lost_counter++;
					if(lost_counter > VISION_LOST_FRAME_COUNTER)
					{
						lost_counter = 0;
						lost = true;
						//cout << "lost = true" << endl;
					}
				}
				else
				{
					lost_counter = 0;
				}
			}
		}
		if (!lost)
		{
			vision.target = roi;
			vision.found = 1;
			vision.x = roi.x - halfWidth;
			vision.y =roi.y - halfHeight;
			vision.z = roi.area() / VISION_TARGET_STD_SIZE.area();
			vision.f = distance(vision.x, vision.y);
			//cout << centerX(roi) << "," << centerY(roi) << endl;
			rectangle(frame, roi, Scalar(0,255,0), 3);
			
			//cout << "SIZE:     " <<  roi.width << "," << roi.height << endl;
		}
		else
		{
			vision.found = 0;
		}
		imshow("vision",frame);
		waitKey(1);
		//t = cvGetTickCount() - t;
		//cout << "**************************** VISION **********************************"<<endl;
		//cout << "TIME COST                                        " << (t/(cvGetTickFrequency()*1000))  << endl;
		//cout << "*********************************************************************"<<endl;
	}
}

/****************************** SCOUT ********************************/
typedef struct Scout
{
	uint8_t rotate,tilt,start_point;
	uint32_t t;
}Scout;

void Halt()
{
	para.fb = para.lr = para.rotate = para.pan = para.tilt = 127;
	para.fire = 0;
}

Scout scout = {127, 127, 127, 0};

void ScoutSM()
{
	scout.rotate = scout.tilt = 127*sin(2*3.1415926*scout.t/(double)SCOUT_PERIOD)+scout.start_point;
	scout.rotate = scale(scout.rotate,0,254,100,154);
	scout.t++;
	if(scout.t>=SCOUT_PERIOD) {
		scout.t=0;
	}
}

void StaticScout()
{
	Halt();
	scout.tilt = 128;
}

int scout_duty_count = 0;
int scout_period_count = 0;
//int scout_ramp_count = 0;
void IntervalScout()
{
	Halt();
	scout_period_count++;
	if(scout_period_count<SCOUT_DUTY_CYCLE)
	{
		scout.rotate = 160;
	}
	else
	{
		scout.rotate = 127;
	}
	scout.tilt = 128;
	if(scout_period_count>SCOUT_INTERVAL)
	{
		scout_period_count = 0;
	}
}

void ScoutContinue()
{
	scout.t=0;
	scout.start_point = para.tilt;
}

void ScoutReset()
{
	scout.t=0;
}

void ScoutAction()
{
	//ScoutSM();
	//StaticScout();
	IntervalScout();
	para.rotate = scout.rotate;
	para.tilt =  scout.tilt;
	para.fb = para.lr = para.pan = 127;
	para.fire = 0;
}

/****************************** WORKING STATE SWITCH MACHINE ********************************/
uint32_t frame_delay_from_lost_to_scout = 0;
uint32_t prepare_period_counter = 0;
typedef enum
{
	PREPARE = 0,
	OBSTACLE_AVOIDANCE = 1,
	//DETETCT = 2,
	TRACKING = 3,
	SCOUT = 4
}WorkingState;

WorkingState working_state = PREPARE;

void WorkingStateSM()
{
	switch(working_state)
	{
		case PREPARE:
		if(prepare_period_counter < PREPARE_PERIOD)
		{
			prepare_period_counter++;
		}
		else
		{
			if(ObstacleClear())
			{
				if(VisionFound())
				{
					ramp_count = 0;
					scout_period_count = 0;
					working_state = TRACKING;
					frame_delay_from_lost_to_scout = 0;
				}
				else
				{
					//ScoutReset();
					//Halt();
					working_state = SCOUT;
				}
			}
			else
			{
				working_state = OBSTACLE_AVOIDANCE;
			}
		}
		break;
		
		case OBSTACLE_AVOIDANCE:
		if(ObstacleClear())
		{
			if(VisionFound())
			{
				ramp_count = 0;
				scout_period_count = 0;
				working_state = TRACKING;
				frame_delay_from_lost_to_scout = 0;
			}
			else
			{
				//ScoutReset();
				working_state = SCOUT;
			}
		}
		else
		{
			scout_period_count = 0;
			working_state = OBSTACLE_AVOIDANCE;
		}
		break;
		
		case TRACKING:
		if(ObstacleClear())
		{
			if(VisionFound())
			{
				//ramp_count = 0;
				working_state = TRACKING;
				scout_period_count = 0;
				frame_delay_from_lost_to_scout = 0;
			}
			else
			{
				scout_period_count = 0;
				working_state = SCOUT;
				//ScoutReset();
				//if(frame_delay_from_lost_to_scout > FRAME_DELAY_FROM_LOST_T0_SCOUT)
				//{
				//	working_state = SCOUT;
					//ScoutContinue();
				//}
				//else
				//{
				//	frame_delay_from_lost_to_scout++;
				//}
			}
		}
		else
		{
			working_state = OBSTACLE_AVOIDANCE;
		}
		break;
		
		case SCOUT:
		if(ObstacleClear())
		{
			if(VisionFound())
			{
				working_state = TRACKING;
				scout_period_count = 0;
				frame_delay_from_lost_to_scout = 0;
			}
			else
			{
				working_state = SCOUT;
			}
		}
		else
		{
			working_state = OBSTACLE_AVOIDANCE;
		}
		break;
	}
	
}

void PrepareAction()
{
	para.fb = para.lr = para.rotate = para.pan = para.tilt = 127;
	para.fire = 0;
}

void Action()
{
	if(working_state == PREPARE)
	{
		cout << "PREPARE"  << endl;
		PrepareAction();
	}
	else if(working_state == OBSTACLE_AVOIDANCE)
	{
		cout << "OBSTACLE_AVOIDANCE"  << endl;
		UltrasAction();
	}
	else if(working_state == TRACKING)
	{
		cout << "TRACKING"  << endl;
		VisionAction();
	}
	else
	{
		cout << "SCOUT"  << endl;
		ScoutAction();
	}
	//cout << (int)para.fb << "," << (int)para.lr << "," << (int)para.rotate << "," << (int)para.pan << ","  << (int)para.tilt << "," << (int)para.fire << endl;
}

int main(int argc, char* argv[])
{
	if(!robot.connect())
	{
		cout << "fail to connect robot!" << endl;
		return -1;
	}
	MyThread ultrasThread, visionThread;
	ultrasThread.create(ultrasThreadFunction,NULL);
	visionThread.create(visionThreadFunction,NULL);
	//double t = 0;
	while(1)
	{
		//t = cvGetTickCount();
		//ScoutSM();
		WorkingStateSM();
		Action();
		//para.tilt = 130;
		Cmd();
		//t = cvGetTickCount() - t;
		//cout << "**************************** ROBOT **********************************"<<endl;
		//cout << "TIME COST                                        " << (t/(cvGetTickFrequency()*1000)) << endl;
		//cout << "*********************************************************************"<<endl;
		usleep(2000);
	}
	return 0;
}
